#!/usr/bin/env python

import roslib; roslib.load_manifest('brain_stuff')
import rospy
import image_geometry
import message_filters
from sensor_msgs.msg import Image, CameraInfo
from std_msgs.msg import Float32
from cv_bridge import CvBridge, CvBridgeError
import cv
import math
import copy
import sys
import array
from PySide.QtCore import *
from PySide.QtGui import *
import smach
import threading

# Convert cv image to qimage...
class IplQImage(QImage):
   def __init__(self, iplimage):
      rgba = cv.CreateMat(iplimage.height, iplimage.width, cv.CV_8UC4)
      cv.Set(rgba, (1,2,3,4))

      cv.MixChannels([iplimage], [rgba], [ (0,0), (1,1), (2,2), (3,3) ] ) # This encoding works because I explicitly convert the imgmsg to cv in rgba
      self.__imagedata = rgba.tostring()
      super(IplQImage,self).__init__(self.__imagedata, iplimage.width, iplimage.height, QImage.Format_RGB32)

class QSignal(QObject):
   signal = Signal()
class MouseSignal(QObject):
   signal = Signal(int, int, int, int)

class Click(smach.State):
    def __init__(self, group_name, buttons):
        smach.State.__init__(self, outcomes=['succeeded', 'failed'])
        self.layout = QVBoxLayout()

        self.button_list = [QPushButton(name) for name in buttons]
        self.return_list = [value for value in buttons.values()]
        map(lambda x: x[0].clicked.connect(lambda: self.callback(x[1])), zip(self.button_list, self.return_list))
        map(self.layout.addWidget, self.button_list)

        self.group = QGroupBox(group_name)
        self.group.setLayout(self.layout)

    def callback(self, value):
        self.return_value = value
        self.group.hide()
        app.quit()

    def execute(self, userdata):
        self.return_value = None

        self.group.show()
        app.exec_()

        return self.return_value

class MyLabel(QLabel):
   def __init__(self):
      super(MyLabel, self).__init__()
      self.mousePress = False
      self.start = None
      self.done = MouseSignal()

   def mousePressEvent(self, event):
      if self.mousePress:
         (ex, ey) = (event.x(), event.y())
         (sx, sy) = self.start
         self.mousePress = False
         self.done.signal.emit(sx, sy, ex, ey)
      else:
         self.start = (event.x(), event.y())
         self.mousePress = True
      event.accept()

   def mouseMoveEvent(self, event):
      event.accept()


class Interface(smach.State):
   def __init__(self):
      # Smach
      smach.State.__init__(self, outcomes=['success', 'aborted'])
      self.userdata = None
      # Flags and global vars
      self.qimage = QImage()
      # OpenCv setup
      self.bridge = CvBridge()
      # Publishers and Subscribers
      self.image_sub = rospy.Subscriber("/wide_stereo/right/image_rect", Image, self.callback)
      rospy.loginfo("Waiting for transform from base_link to wide_stereo_optical_frame")
      # Pyside
      self.imageReady = QSignal()
      self.imageLock = threading.Lock()
      self.group = QGroupBox('group1')
      self.label2 = MyLabel()

      self.button = QPushButton('done')

      vbox = QVBoxLayout()
      vbox.addWidget(self.label2)
      vbox.addWidget(self.button)
      self.group.setLayout(vbox)

   def done(self, sx=None, sy=None, ex=None, ey=None):
      self.userdata = ((sx, sy), (ex, ey))
      if sx == None:
         self.userdata = (None, None)
         
      self.group.hide()
      app.quit()

   def execute(self, userdata):
      self.imageReady.signal.connect(self.showImage)
      self.button.clicked.connect(self.done)
      self.label2.done.signal.connect(self.done)
      self.group.show()
      app.exec_()
      rospy.loginfo("app executed")
      (start, end) = self.userdata
      if start == None:
         return 'aborted'
      (sx, sy) = start
      (ex, ey) = end
      rospy.loginfo("HIGH: {0} {1}".format(max(sx, ex), max(sy, ey)))
      rospy.loginfo("LOW: {0} {1}".format(min(sx, ex), min(sy, ey)))
      return 'success' 

   def showImage(self):
      self.imageLock.acquire()
      qpixmap = QPixmap.fromImage(self.qimage)
      self.label2.setPixmap(qpixmap)
      self.imageLock.release()

      
   def callback(self, data):
      if rospy.is_shutdown():
         return
      try:
         cv_image = self.bridge.imgmsg_to_cv(data, "rgba8")
      except CvBridgeError, e:
         print e
         return
      self.imageLock.acquire()
      self.qimage = IplQImage(cv_image)
      self.imageLock.release()
      self.imageReady.signal.emit()

class TurnKnob(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded'])

        self.layout = QVBoxLayout()
        self.dial = QDial()
        self.dial.valueChanged.connect(self.callback)
        self.dial.setNotchesVisible(True)
        self.dial.setWrapping(True)
        self.layout.addWidget(self.dial)

        self.button = QPushButton('done')
        self.button.clicked.connect(self.done)
        self.layout.addWidget(self.button)

        self.group = QGroupBox('knob control')
        self.group.setLayout(self.layout)

        self.pub = rospy.Publisher('knob/value', Float32)

    def callback(self, value):
        self.pub.publish(float(value) / 99.0 * 3.14159 * 2.0)

    def done(self):
        self.group.hide()
        app.quit()

    def execute(self, userdata):
        self.group.show()
        app.exec_()

        return 'succeeded'





if __name__== "__main__":
   rospy.init_node('test_interface')

   app = QApplication(sys.argv)
   machine = smach.StateMachine(outcomes = ['success', 'failed'])
   with machine:
      smach.StateMachine.add('START', Click('Find', {'knobs':'succeeded', 'quit':'failed'}), transitions={'succeeded':'INTERFACE', 'failed':'failed'})
      smach.StateMachine.add("INTERFACE", Interface(), transitions={'aborted':'TURN_KNOB', 'success':'TURN_KNOB'})
      smach.StateMachine.add('TURN_KNOB', TurnKnob(), transitions={'succeeded':'START'})
   try:
      machine.execute()
   except KeyboardInterrupt:
      exit()


